/// Alvaro Cuno, Mar2008
/// Dual DualQuaternion definition file

#ifndef _DUALDualQuaternion_H_
#define _DUALDualQuaternion_H_

#include <cmath>
#include <cmath>
#include <cassert>
#include <iostream>

using std::cout;
using std::endl;

template <class Number>
class DualQuaternion {
   
public:
   /// Default constructor
   DualQuaternion() {
      q[0]=1.0; q[1]=q[2]=q[3]=0.0;
      q[4]=0.0; q[5]=q[6]=q[7]=0.0;
   }
   /// Copy constructor
   DualQuaternion(const DualQuaternion &Q) {
      *this = Q;
   }
   /// Constructor
   //~ DualQuaternion(Number q0, Number q1, Number q2, Number q3) {
      
   //~ }
   /// Assignment operator
   const DualQuaternion& operator=(const DualQuaternion &Q) {
      for (int i=0; i<8; ++i) q[i] = Q.q[i];
      return *this;
   }
   /// DualQuaternion sum operator
   DualQuaternion operator+(const DualQuaternion &Q) const {
      DualQuaternion R(*this);
      for (int i=0; i<8; ++i) R.q[i]+=Q.q[i]; 
      return R;
   }
   /// DualQuaternion product operator
   DualQuaternion operator*(const DualQuaternion &Q) const {
      DualQuaternion R; const DualQuaternion &P = *this;
      //~ R.q[0]=P.q[0]*Q.q[0] - P.q[1]*Q.q[1] - P.q[2]*Q.q[2] - P.q[3]*Q.q[3];
      //~ R.q[1]=P.q[1]*Q.q[0] + P.q[0]*Q.q[1] + P.q[2]*Q.q[3] - P.q[3]*Q.q[2];
      //~ R.q[2]=P.q[2]*Q.q[0] + P.q[0]*Q.q[2] + P.q[3]*Q.q[1] - P.q[1]*Q.q[3];
      //~ R.q[3]=P.q[3]*Q.q[0] + P.q[0]*Q.q[3] + P.q[1]*Q.q[2] - P.q[2]*Q.q[1];
      return R;
   }
   /// read-only specific field operator
   const Number operator[](int i) const {
      assert(i<8);
      return q[i];
   }
   
   /// output operator
   template <class T>
   friend ostream &operator<<(ostream &, const DualQuaternion<T> &);
   /// input operator
   template <class T>
   friend istream &operator>>(istream &, const DualQuaternion<T> &);
   
   /// Instances a DualQuaternion given an unit axis and angle
   //~ static DualQuaternion fromAxisAngle(Number x, Number y, Number z, Number angle) {
      //~ const Number sina = sin(angle*0.5); /// sina = sin(angle/2)
      //~ const Number &cosa = cos(angle*0.5); /// cosa = cos(angle/2)
      //~ return DualQuaternion(cosa, x*sina, y*sina, z*sina);
   //~ }
   
private:
   Number q[8]; // The DualQuaternion comnponents
};

/// Point output operator
template <class Number>
ostream &operator<<(ostream &out, const DualQuaternion<Number> &Q) {
   for (int i=0; i<8; ++i) out<<Q.q[i]<<" ";
   return out;
}

/// Point input operator
template <class Number>
istream &operator>>(istream &in, const DualQuaternion<Number> &Q) {
   for (int i=0; i<8; ++i) in>>Q.q[i];
   return in;
}

//~ /// dot product of two DualQuaternions
//~ template <class Number>
//~ Number dot(const DualQuaternion<Number> &Q, const DualQuaternion<Number> &R) {
   //~ return Q[0]*R[0] + Q[1]*R[1] + Q[2]*R[2] + Q[3]*R[3];
//~ }

//~ /// DualQuaternion conjugate
//~ template <class Number>
//~ DualQuaternion<Number> conjugate(const DualQuaternion<Number> &Q) {
   //~ return DualQuaternion<Number> (Q[0], -Q[1], -Q[2], -Q[3]);
//~ }

#endif
